package org.hs.phd.odi.tracking.tracker.comaniciu;

import java.awt.Polygon;
import java.awt.Rectangle;
import java.awt.geom.Point2D;
import java.util.Arrays;

import org.hs.phd.odi.tracking.tracker.Histogram;
import org.hs.phd.odi.tracking.tracker.SimilarityMeasure;
import org.hs.phd.odi.tracking.tracker.TargetCandidate;
import org.hs.phd.odi.tracking.tracker.TrackWindow;
import org.hs.phd.odi.tracking.tracker.Tracker;
import org.hs.phd.odi.tracking.tracker.TrackerUtils;
import org.hs.phd.odi.tracking.tracker.histogram.Weighted3DHistogram;
import org.hs.phd.odi.tracking.tracker.kernel.EpanechnikovKernel;
import org.hs.phd.odi.tracking.tracker.similaritymeasure.BhattacharyaCoefficientSimilartyMeasure;
import org.hs.phd.odi.tracking.tracker.trackwindow.RegularGridTrackWindow;
import org.jfree.chart.ChartFactory;
import org.jfree.chart.JFreeChart;
import org.jfree.chart.plot.PlotOrientation;
import org.jfree.data.time.TimeSeries;
import org.jfree.data.time.TimeSeriesCollection;
import org.jfree.data.xy.DefaultIntervalXYDataset;

import com.googlecode.javacv.cpp.opencv_core;
import com.googlecode.javacv.cpp.opencv_highgui;
import com.googlecode.javacv.cpp.opencv_core.IplImage;

public class ComaniciuTracker implements Tracker {

	private TrackWindow currentTrackWindow;
	private Histogram targetHistogram;
	private final SimilarityMeasure similarityMeasure;
	private final TrackWindow prototypeTrackWindow;
	
	private IplImage frameCopyForVisualization;
	private boolean isVisulizationEnabled;
	private int visualizationWaitMillis = 30;

	public ComaniciuTracker( Histogram histogram, SimilarityMeasure similarityMeasure, TrackWindow prototypeTrackWindow) {
		this.similarityMeasure = similarityMeasure;
		this.prototypeTrackWindow = prototypeTrackWindow;
		targetHistogram = histogram;
	}

	public ComaniciuTracker() {
		this(new Weighted3DHistogram(32, 32, 16, new EpanechnikovKernel()), 
				new BhattacharyaCoefficientSimilartyMeasure(), new RegularGridTrackWindow() );
	}

	@Override
	public TrackWindow init(Polygon initialTrackShape, IplImage initialFrame) {
		targetHistogram.calculateFrom(initialFrame, initialTrackShape);
		currentTrackWindow = prototypeTrackWindow.getCopy();
		currentTrackWindow.moveToBestMatchingLocationFor( initialTrackShape );
		
		if( isVisulizationEnabled ){
			TrackerUtils.printArrayNonZeroes("Target", targetHistogram.getData());
			visualizeInitialTrackShape(initialTrackShape, initialFrame);
		}
		
		return currentTrackWindow;
	}


	@Override
	public TrackWindow track(IplImage frame) {
		TargetCandidate candidate0 = new TargetCandidate( frame, 
				currentTrackWindow.getCopy(), targetHistogram, similarityMeasure);

		if( isVisulizationEnabled ){
			debugCandidate(candidate0);
		}
		
		boolean isCandidatesCloseEnough = false;

		while (!isCandidatesCloseEnough) {

			Point2D.Float y1 = candidate0.calculateNextMeanshiftPosition();

			TargetCandidate candidate1 = candidate0.getMovedCopy(y1);
			

			while (!(isCandidatesCloseEnough = candidate1.isCloseEnough(candidate0)) && candidate1.getTargetSimilarity() < candidate0.getTargetSimilarity()) {
				candidate1.moveTowards(candidate0, 0.5F);
			}

			if( isVisulizationEnabled ){
				debugCandidate(candidate1);
			}
			
			candidate1.copyTo(candidate0);

		}

		currentTrackWindow = candidate0.getTrackWindow();

		if( isVisulizationEnabled ){
			debugCandidateSelected();
			visualizeCandidate(frame, candidate0);
		}
		

		return currentTrackWindow;
	}

	private void debugCandidateSelected() {
		System.out.println("Candidate selected as next track position.");
		System.out.println("----------------------------------------------");
		System.out.println();
	}

	private void debugCandidate(TargetCandidate candidate) {
		System.out.printf("Center: [%f, %f], Similarity: %f \n", candidate.getTrackWindow().getCenter().x, candidate.getTrackWindow().getCenter().y, candidate.getTargetSimilarity());
		TrackerUtils.printArrayNonZeroes("Target", candidate.getTargetHistogram().getData());
		TrackerUtils.printArrayNonZeroes("Candidate", candidate.getCandidateHistogram().getData());
		
//		Histogram recalculatedCandidateHistogram = candidate.getTargetHistogram().getCopy();
//		Rectangle trackShape = candidate.getTrackWindow().getAsPolygon().getBounds();
//		recalculatedCandidateHistogram.calculateFrom( candidate.getFrame(), trackShape);
//		ComaniciuUtils.printArray("Recalculated Candidate for " + candidate.getTrackWindow().getCenter(), recalculatedCandidateHistogram.getData());
		System.out.println();
	}


	private void visualizeInitialTrackShape(Polygon initialTrackShape, IplImage initialFrame) {
		Rectangle bounds = initialTrackShape.getBounds();
		opencv_core.cvRectangle(initialFrame, opencv_core.cvPoint(bounds.x, bounds.y), 
				opencv_core.cvPoint(bounds.x + bounds.width, bounds.y + bounds.height), 
				opencv_core.CV_RGB(0, 255, 0), 1, opencv_core.CV_AA, 0);
		opencv_highgui.cvShowImage("tracking input", initialFrame);
		opencv_highgui.cvWaitKey(visualizationWaitMillis);
	}

	private void visualizeCandidate(IplImage frame, TargetCandidate candidate0) {

		visualizeHistogram(candidate0);
		
		
		Rectangle bounds = candidate0.getTrackWindow().getAsPolygon().getBounds();
		
		if( frameCopyForVisualization == null ){
			frameCopyForVisualization = frame.clone();
		}else{
			opencv_core.cvCopy(frame, frameCopyForVisualization);
		}
		opencv_core.cvRectangle(frameCopyForVisualization, opencv_core.cvPoint(bounds.x, bounds.y), 
				opencv_core.cvPoint(bounds.x + bounds.width, bounds.y + bounds.height), 
				opencv_core.CV_RGB(255, 0, 150), 1, opencv_core.CV_AA, 0);
		opencv_highgui.cvShowImage("tracking input", frameCopyForVisualization);
		opencv_highgui.cvWaitKey(visualizationWaitMillis );
	}

	private void visualizeHistogram(TargetCandidate candidate0) {

//		TimeSeriesCollection dataset = new TimeSeriesCollection();
//		TimeSeries timeSeriesTarget = new TimeSeries("Target");
//		timeSeriesTarget.add(new TimeSer)
//		dataset.addSeries(timeSeriesTarget);
//		
//		JFreeChart chart = ChartFactory.createHistogram("Histogram", "bins", "", dataset, PlotOrientation.VERTICAL, false, false, false);
//		
	}

	public boolean isVisulizationEnabled() {
		return isVisulizationEnabled;
	}

	public void setVisulizationEnabled(boolean isVisulizationEnabled) {
		this.isVisulizationEnabled = isVisulizationEnabled;
	}

	public int getVisualizationWaitMillis() {
		return visualizationWaitMillis;
	}

	public void setVisualizationWaitMillis(int visualizationWaitMillis) {
		this.visualizationWaitMillis = visualizationWaitMillis;
	}


}
